﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Server.Core.Filter
{
    class KalmanFilter2 : IFilter
    {
        private double q, r, p, k, x = 0;

        public KalmanFilter2(double _q, double _r, double _p)
        {
            q = _q;
            r = _r;
            p = _p;
        }

        public double processData(double input)
        {
            if (x == 0)
            {
                x = input;
            }
            p = p + q;
            k = p / (p + r);
            x = x + k * (input - x);
            p = (1 - k) * p;
            return x;
        }
    }
}
